00001 CarPhysics::~CarPhysics()
00002 {
00003
00004 }
00005
00006 CarPhysics::CarPhysics(Vehicle &vehicle)
00007 {
00008
00009 _chassisHgt = 0;
00010 _steering = 0;
00011 _accel = 0;
00012 _speed = 0;
00013
00014 _traction = 1;
00015
00016 _gearMod = 1;
00017
00018 _carBody = 0;
00019
00020 _wheelBody[0] = 0;
00021 _wheelBody[1] = 0;
00022
00023 _wheelBody[2] = 0;
00024 _wheelBody[3] = 0;
00025
00026 _joint[0] = 0;
00027 _joint[1] = 0;
00028 _joint[2] = 0;
00029 _joint[3] = 0;
00030
00031 _carSpace = 0;
00032
00033 _wheelGeom[0] = 0;
00034 _wheelGeom[1] = 0;
00035 _wheelGeom[2] = 0;
00036 _wheelGeom[3] = 0;
00037 OSGvehicle=vehicle;
00038 }
00039
00040
00041
00042 bool CarPhysics::create( const CarData* carDataPtr ,osg::ref_ptr<osg::PositionAttitudeTransform> objectPAT)
00043 {
00044 if ( carDataPtr == NULL)
00045 return false;
00046
00047
00048 if (_world == 0 || _space == 0)
00049 return false;
00050
00051
00052 _pCarData = carDataPtr;
00053
00054
00055 _chassisHgt = 0.2;
00056
00057
00058 double chassisMass = _pCarData->mass;
00059 double len = _pCarData->length;
00060 double wdt = _pCarData->width;
00061 double hgt = _pCarData->height;
00062
00063 _carBody = dBodyCreate (_world);
00064
00065
00066 dMass m;
00067 dMassSetBoxTotal(&m, chassisMass*PHYS_MASS_SCALE,len, wdt, _chassisHgt);
00068 dBodySetMass (_carBody, &m);
00069
00070
00071 _carSpace = dSimpleSpaceCreate(_space);
00072
00073
00074 dGeomID chassis = dCreateBox(0, len, wdt, _chassisHgt);
00075 dMatrix3 RI;
00076 dRSetIdentity (RI);
00077 addGeom(chassis, 0, 0, 0, RI);
00078
00079
00080 dGeomID fuseFront = dCreateBox(0, len*0.8, wdt*1.1, hgt*0.8);
00081 dMatrix3 R2;
00082 dRFromEulerAngles(R2, 0, DEG2RAD(-5), 0);
00083 addGeom(fuseFront, (len*0.2), 0, _pCarData->chassisOffset, R2);
00084
00085
00086 dGeomID fuseRear = dCreateBox(0, len*0.6, wdt*1.3, hgt*0.8);
00087 dMatrix3 R1;
00088 dRFromEulerAngles(R1, 0, DEG2RAD(5), 0);
00089 addGeom(fuseRear, (-len*0.3), 0, _pCarData->chassisOffset, R1);
00090
00091
00092
00093 for (int i = 0; i != 4; ++i)
00094 {
00095 _wheelBody[i] = dBodyCreate (_world);
00096
00097
00098 dQuaternion q;
00099 dQFromAxisAndAngle (q, 1, 0, 0 , M_PI*0.5);
00100 dBodySetQuaternion (_wheelBody[i], q);
00101
00102
00103 dMassSetSphereTotal (&m, _pCarData->wheelMass*PHYS_MASS_SCALE, _pCarData->wheelRadius);
00104 dBodySetMass (_wheelBody[i], &m);
00105
00106
00107 _wheelGeom[i] = dCreateSphere (0, _pCarData->wheelRadius);
00108 _wheelData.slip = WHEEL_SLIP;
00109 dGeomSetData(_wheelGeom[i], (void*)&_wheelData);
00110 dGeomSetBody (_wheelGeom[i], _wheelBody[i]);
00111
00112 dSpaceAdd(_carSpace, _wheelGeom[i]);
00113 }
00114
00115
00116 osg::Vec3d position=objectPAT->getPosition();
00117
00118 setPosition(position[0],position[1],position[2]);
00119
00120
00121
00122 for (i = 0; i != 4; i++)
00123 {
00124
00125 _joint[i] = dJointCreateHinge2 (_world, 0);
00126
00127
00128 dJointAttach(_joint[i], _carBody, _wheelBody[i]);
00129
00130 const dReal *a = dBodyGetPosition (_wheelBody[i]);
00131
00132
00133 dJointSetHinge2Anchor (_joint[i], a[0], a[1], a[2]);
00134
00135
00136 dJointSetHinge2Axis1 (_joint[i], 0, 0, 1);
00137
00138
00139 dJointSetHinge2Axis2 (_joint[i], 0, 1, 0);
00140 }
00141
00142
00143 osg::Quat quat =objectPAT->getAttitude();
00144 dReal* odeQuat= new dReal[4];
00145 quat.getRotate(odeQuat[0],odeQuat[1],odeQuat[2],odeQuat[3]);
00146 double angle=RAD2DEG(odeQuat[0]);
00147 angle=-angle;
00148 setRotation(0.0,0.0,angle);
00149
00150
00151 for (i = 0; i != 2; ++i)
00152 {
00153
00154 dJointSetHinge2Param (_joint[i], dParamSuspensionERP,0.6);
00155 dJointSetHinge2Param (_joint[i], dParamSuspensionCFM,0.05);
00156 }
00157
00158 for (i = 2; i != 4; ++i)
00159 {
00160
00161 dJointSetHinge2Param (_joint[i], dParamSuspensionERP,0.6);
00162 dJointSetHinge2Param (_joint[i], dParamSuspensionCFM,0.05);
00163 }
00164
00165
00166
00167 for (i = 2; i != 4; ++i)
00168 {
00169 dJointSetHinge2Param (_joint[i], dParamLoStop, 0);
00170 dJointSetHinge2Param (_joint[i], dParamHiStop, 0);
00171 }
00172
00173
00174
00175 _steering = 0;
00176 _accel = 0;
00177 _speed = 0;
00178 _traction = 1;
00179
00180 }
00181
00182
00183 double CarPhysics::update(double step)
00184 {
00185 if (_world == 0 || _space == 0)
00186 return 0;
00187
00188
00189 dReal drag = 1.5 * _speed * PHYS_FORCE_SCALE;
00190
00191
00192 _speed += _accel - drag;
00193
00194 if (_speed < 0) _speed = 0;
00195
00196
00197 int startWheel = 2;
00198 int endWheel = 4;
00199
00200
00201 dReal totalForce = _pCarData->power * PHYS_FORCE_SCALE;
00202 if (_accel < 0)
00203 {
00204 totalForce = 30;
00205 }
00206 dReal force = totalForce/(endWheel - startWheel);
00207
00208
00209 for(int i = startWheel; i != endWheel; ++i)
00210 {
00211 dJointSetHinge2Param (_joint[i], dParamVel2, - _gearMod * _speed);
00212 dJointSetHinge2Param (_joint[i], dParamFMax2, force);
00213 }
00214
00215
00216 dReal langle = dJointGetHinge2Angle1 (_joint[0]);
00217
00218
00219 dReal vl = (_steering * STEER_LIMIT_ANGLE) - langle;
00220 if (vl > 0.1) vl = 0.1;
00221 if (vl < -0.1) vl = -0.1;
00222 vl *= 10.0;
00223
00224 dJointSetHinge2Param (_joint[0],dParamVel, vl);
00225 dJointSetHinge2Param (_joint[0],dParamFMax, 0.2);
00226 dJointSetHinge2Param (_joint[0],dParamLoStop, -STEER_LIMIT_ANGLE);
00227 dJointSetHinge2Param (_joint[0],dParamHiStop, STEER_LIMIT_ANGLE);
00228 dJointSetHinge2Param (_joint[0],dParamFudgeFactor, 0.1);
00229
00230
00231 dReal rangle = dJointGetHinge2Angle1 (_joint[1]);
00232
00233 dReal vr = (_steering*STEER_LIMIT_ANGLE) - rangle;
00234 if (vr > 0.1) vr = 0.1;
00235 if (vr < -0.1) vr = -0.1;
00236 vr *= 10.0;
00237
00238 dJointSetHinge2Param (_joint[1],dParamVel, vr);
00239 dJointSetHinge2Param (_joint[1],dParamFMax, 0.2);
00240 dJointSetHinge2Param (_joint[1],dParamLoStop, -STEER_LIMIT_ANGLE);
00241 dJointSetHinge2Param (_joint[1],dParamHiStop, STEER_LIMIT_ANGLE);
00242 dJointSetHinge2Param (_joint[1],dParamFudgeFactor, 0.1);
00243
00244
00245
00246 return _speed;
00247 }
00248
00249
00250 void CarPhysics::setGear(int gear)
00251 {
00252 if (gear < -1 || gear > 1)
00253 return;
00254
00255 if (gear == -1)
00256 _gearMod = -0.1;
00257 else
00258 _gearMod = gear;
00259 }
00260
00261
00262
00263 double CarPhysics::getSpeed()
00264 {
00265
00266
00267 static Vector3 velocity;
00268 const dReal* vel = dBodyGetLinearVel(_carBody);
00269 velocity.set(vel[0], vel[1], vel[2]);
00270
00271 return (velocity.mag() * 3.6);
00272 }
00273
00274
00275
00276 const Vector3& CarPhysics::getPosition()
00277 {
00278 const double *pos = getGeomPosition(_carGeoms[0]);
00279
00280
00281 static Vector3 retPos;
00282
00283 retPos.set(pos[0], pos[1], pos[2]);
00284
00285 return (retPos);
00286 }
00287
00288
00289
00290 const double* CarPhysics::getRotation()
00291 {
00292 return getGeomRotation(_carGeoms[0]);
00293 }
00294
00295
00296
00297 void CarPhysics::setPosition(double x, double y, double z)
00298 {
00299
00300 positionCar(x,y,z);
00301 }
00302
00303
00304
00305 void CarPhysics::setRotation(double x, double y, double z)
00306 {
00307
00308 dBodySetLinearVel(_carBody,0,0, 0);
00309 dBodySetAngularVel(_carBody,0,0,0);
00310 for(int i = 0; i != 4; ++i)
00311 {
00312 dBodySetLinearVel(_wheelBody[i],0,0, 0);
00313 dBodySetAngularVel(_wheelBody[i],0,0,0);
00314 }
00315
00316
00317 double phi = DEG2RAD(x);
00318
00319 double theta = DEG2RAD(y);
00320 double psi = DEG2RAD(z);
00321 dMatrix3 R;
00322 dRFromEulerAngles (R, phi, theta, psi);
00323 dBodySetRotation(_carBody, R);
00324 }
00325
00326 void CarPhysics::positionCar(double x, double y, double z)
00327 {
00328 dMass m;
00329
00330 dBodySetLinearVel(_carBody,0,0, 0);
00331 dBodySetAngularVel(_carBody,0,0,0);
00332 for(int i = 0; i != 4; ++i)
00333 {
00334 dBodySetLinearVel(_wheelBody[i],0,0, 0);
00335 dBodySetAngularVel(_wheelBody[i],0,0,0);
00336 }
00337
00338 dBodySetPosition(_carBody, x, y, z + _pCarData->chassisOffset);
00339
00340
00341 double xoff = _pCarData->length*0.4;
00342 double yoff = _pCarData->width*0.5;
00343 double zoff = _pCarData->wheelRadius;
00344
00345
00346 dBodySetPosition (_wheelBody[0], x+xoff, y+yoff, z+zoff);
00347
00348 dBodySetPosition (_wheelBody[1], x+xoff, y-yoff, z+zoff);
00349
00350 dBodySetPosition (_wheelBody[2], x-xoff, y+yoff, z+zoff);
00351
00352 dBodySetPosition (_wheelBody[3], x-xoff, y-yoff, z+zoff);
00353 }
00354
00355 const double* CarPhysics::getGeomPosition(dGeomID geom)
00356 {
00357 const dReal *pos = dGeomGetPosition (geom);
00358 if (dGeomGetClass(geom) != dGeomTransformClass)
00359 return pos;
00360
00361
00362 const dReal *R = dGeomGetRotation (geom);
00363
00364 dGeomID g2 = dGeomTransformGetGeom (geom);
00365 const dReal *pos2 = dGeomGetPosition (g2);
00366 const dReal *R2 = dGeomGetRotation (g2);
00367
00368 static dVector3 actual_pos;
00369 dMULTIPLY0_331 (actual_pos,R,pos2);
00370 actual_pos[0] += pos[0];
00371 actual_pos[1] += pos[1];
00372 actual_pos[2] += pos[2];
00373
00374 return actual_pos;
00375
00376 }
00377
00378
00379 const double* CarPhysics::getGeomRotation(dGeomID geom)
00380 {
00381 const dReal *R = dGeomGetRotation (geom);
00382 if (dGeomGetClass(geom) != dGeomTransformClass)
00383 return R;
00384
00385
00386 dGeomID g2 = dGeomTransformGetGeom (geom);
00387 const dReal *pos2 = dGeomGetPosition (g2);
00388 const dReal *R2 = dGeomGetRotation (g2);
00389 static dMatrix3 actual_R;
00390 dMULTIPLY0_333 (actual_R, R, R2);
00391
00392 double RR[12];
00393 memcpy(RR, actual_R, sizeof(dMatrix3));
00394
00395 return actual_R;
00396 }
00397
00398 void CarPhysics::mySetPosition(){
00399
00400 static int index=0;
00401
00402
00403 osg::Vec3d position=OSGvehicle.getPath()->getPointVector()[index];
00404
00405 setPosition(position[0],position[1],position[2]);
00406
00407 osg::Quat quat =OSGvehicle.getObjectPATvector()[index]->objectPAT->getAttitude();
00408 dReal* odeQuat= new dReal[4];
00409 quat.getRotate(odeQuat[0],odeQuat[1],odeQuat[2],odeQuat[3]);
00410 double angle=RAD2DEG(odeQuat[0]);
00411 angle=-angle;
00412 setRotation(0.0,0.0,angle);
00413
00414 }
00415
00416
00417 void CarPhysics::addGeom(dGeomID geom, dReal xoff, dReal yoff, dReal zoff, dMatrix3 R)
00418 {
00419
00420 dGeomID xform = dCreateGeomTransform(_carSpace);
00421 dGeomTransformSetCleanup(xform, 1);
00422 dGeomTransformSetInfo(xform, 0);
00423
00424
00425 dGeomTransformSetGeom(xform, geom);
00426
00427 dGeomSetBody(xform, _carBody);
00428
00429
00430 dGeomSetPosition(geom, xoff, yoff, zoff);
00431 dGeomSetRotation(geom, R);
00432
00433
00434 _carGeoms.push_back(xform);
00435 }
00436
00437
00438
00439
00440 void CarPhysics::odeDraw()
00441 {
00442
00443 for (int i = 0; i !=(int) _carGeoms.size(); ++i)
00444 {
00445
00446 const dReal* vehicleQuat = dBodyGetQuaternion (_carBody);
00447 OSGvehicle.getObjectPAT()->setAttitude (osg::Quat(vehicleQuat[1], vehicleQuat[2],
00448 vehicleQuat[3], vehicleQuat[0]));
00449 const dReal* vehicleVec = dBodyGetPosition (_carBody);
00450 OSGvehicle.getObjectPAT()->setPosition (osg::Vec3 (vehicleVec[0], vehicleVec[1], vehicleVec[2]));
00451 }
00452
00453
00454 for (i=0; i != 4; i++) {
00455
00456 const dReal* vehicleQuat = dBodyGetQuaternion (_wheelBody[i]);
00457 OSGvehicle.wheelObjects[i].getPAT()->setAttitude (osg::Quat(vehicleQuat[1], vehicleQuat[2],
00458 vehicleQuat[3], vehicleQuat[0]));
00459 const dReal* vehicleVec = dBodyGetPosition (_wheelBody[i]);
00460 OSGvehicle.wheelObjects[i].getPAT()->setPosition (osg::Vec3 (vehicleVec[0], vehicleVec[1], vehicleVec[2]));
00461 }
00462
00463
00464 }
00465
00466 bool PhysicsEventHandler::handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter& aa)
00467 {
00468
00469 if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00470 && (ea.getKey() == 'W' || ea.getKey() == 'w'))
00471 {
00472 _throttleSet=true;
00473 return true;
00474 }
00475 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00476 && (ea.getKey() == 'W' || ea.getKey() == 'w'))
00477 {
00478 _throttleSet=false;
00479 return true;
00480 }
00481
00482 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00483 && (ea.getKey() == 's' || ea.getKey() == 'S'))
00484 {
00485 _brakeSet=true;
00486 return true;
00487 }
00488 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00489 && (ea.getKey() == 's' || ea.getKey() == 'S'))
00490 {
00491 _brakeSet=false;
00492 return true;
00493 }
00494 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00495 && (ea.getKey() == 'a' || ea.getKey() == 'A'))
00496 {
00497 _steerLeftSet=true;
00498 return true;
00499 }
00500 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00501 && (ea.getKey() == 'a' || ea.getKey() == 'A'))
00502 {
00503 _steerLeftSet=false;
00504 return true;
00505 }
00506 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00507 && (ea.getKey() == 'd' || ea.getKey() == 'D'))
00508 {
00509 _steerRightSet=true;
00510 return true;
00511 }
00512 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00513 && (ea.getKey() == 'd' || ea.getKey() == 'D'))
00514 {
00515 _steerRightSet=false;
00516 return true;
00517 }
00518 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00519 && (ea.getKey() == 'x' || ea.getKey() == 'X'))
00520 {
00521 _stop=true;
00522 return true;
00523 }
00524 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00525 && (ea.getKey() == 'x' || ea.getKey() == 'X'))
00526 {
00527 _stop=false;
00528 return true;
00529 }
00530 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00531 && (ea.getKey() == 'b' || ea.getKey() == 'B'))
00532 {
00533 _brakeSet=true;
00534 return true;
00535 }
00536 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00537 && (ea.getKey() == 'b' || ea.getKey() == 'B'))
00538 {
00539 _brakeSet=false;
00540 return true;
00541 }
00542 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00543 && (ea.getKey() == 'q' || ea.getKey() == 'Q'))
00544 {
00545 _brakeSet=true;
00546 return true;
00547 }
00548 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00549 && (ea.getKey() == 'q' || ea.getKey() == 'Q'))
00550 {
00551 _brakeSet=false;
00552 return true;
00553 }
00554
00555 else{
00556 return false;
00557 }
00558 }
00559
00560
00561 Vehicle* CarPhysics::getOSGVehicle(){
00562 return &OSGvehicle;
00563 }